﻿#include "gait_math.h"
#include "eso.h"
#include "locomotion_header.h"
#include "include.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#if RUN_PI
#include "PositionVelocityEstimator.h"
#endif
GAIT_SHEC gait;
_GAIT_WALK walk_gait;
int walk_init = 0;
#define WALK_FAKE_TEST 0//末锟剿诧拷锟斤拷锟斤拷锟矫伙拷锟斤拷锟斤拷
#define FAKE_EXP_ZMP_TEST 0//直锟接革拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷位锟斤拷

#if GAIT_TEST_MODE
#define ONE_LEG_FAKE_TEST 1//锟斤拷锟斤拷锟斤拷锟斤拷锟狡讹拷  锟斤拷锟斤拷
#define ONE_LEG_TRIG_TEST 1//锟斤拷锟斤拷锟斤拷夭锟斤拷锟? 锟斤拷锟斤拷
#else
#define ONE_LEG_FAKE_TEST 0//锟斤拷锟斤拷锟斤拷锟斤拷锟狡讹拷  锟斤拷锟斤拷
#define ONE_LEG_TRIG_TEST 0//锟斤拷锟斤拷锟斤拷夭锟斤拷锟? 锟斤拷锟斤拷
#endif
#define WALK_SECH_ROUND 1//使锟斤拷锟斤拷锟节碉拷锟斤拷

#define EN_DOUBLE_PASS 0

float cog_pid[2] = { 35,35 };//锟斤拷锟侥轨迹锟斤拷锟斤拷锟斤拷锟?float cog_pidd[2]={45,45};
float cog_pidd[2] = { 10,10 };
float cog_pidnn[2] = { 320,200 };//锟斤拷锟侥轨迹锟斤拷锟斤拷锟斤拷锟
//float cog_pidnn[2]={100,150};//锟斤拷锟侥轨迹锟斤拷锟斤拷锟斤拷锟?
float cog_piddnn[2] = { 86,86 };

float k_walk_trig_spd[2] = { 0.18,0.325 };//{0.7,0.35};
float fast_in_trig_check_time = 0.15;
float scale_p[2] = { 1,1 };
float walk_sw_time = 0.05;
float scale_end_off = 0.8;
float cog_reach_time_check = 0.01;
float min_yaw_space = 0.0168;
float time_reach_scale = 1.2;
float time_reach_scale_n = 0.35;
char cog_sel = 0;

END_POS test_leg[4];
char test_leg_ground[4];
END_POS point;
END_POS vector_p;
END_POS tar_zmp_fake;
float yaw_line;
float linetst[2];
float test_dis;
char one_leg_trig_id = 99;
float tst_spd[3] = { 0,0,0 };
float att_per_trig[3] = { 3,2,0 };//{3,1.5,0};



void check_leg_need_move(void)//锟叫讹拷锟角凤拷锟斤拷要停止锟斤拷锟侥碉拷锟斤拷锟饺讹拷
{
	char i = 0, j = 0, k = 0;
	float id_buf_space[4];
	float id_buf_dis_space[4];
	for (i = 0; i < 4; i++)
	{
		if (vmc[i].ground)
		{
			walk_gait.out_ws_flag[i] = check_leg_out_space(i, walk_gait.work_space_band, &walk_gait.work_space[i]);//锟斤拷锟姐工锟斤拷锟秸硷拷
			if (walk_gait.out_ws_flag[i])
			{
				id_buf_dis_space[k] = walk_gait.work_space[i];
				id_buf_space[k] = i;
				k++;
			}

		}
	}

	walk_gait.need_trig = 0;
	if (k > 0)//锟斤拷锟斤拷锟斤拷锟斤拷锟秸硷拷
		walk_gait.need_trig = 1;

	float dis = sqrt(pow(walk_gait.tar_zmp_n.x - walk_gait.now_zmp_n.x, 2) + pow(walk_gait.tar_zmp_n.y - walk_gait.now_zmp_n.y, 2));
	if (fabs(dis) > walk_gait.cog_reach_dead)//COG锟斤拷支锟斤拷锟斤拷锟斤拷锟斤拷锟侥撅拷锟斤拷洗锟?
		walk_gait.need_trig = 1;
}

END_POS cog_planner_per_tirg(char per_trig_id)//预锟斤拷锟斤拷时锟斤拷COG锟芥划
{
	END_POS vector_spd;
	END_POS tar_cog_pos;
	float tar_dx = vmc_all.param.tar_spd_use_rc.x;
	float tar_dy = vmc_all.param.tar_spd_use_rc.y;
	float spd = sqrt(tar_dx*tar_dx + tar_dy * tar_dy);
	float spd_yaw = cal_yaw_xy(tar_dx, tar_dy);//锟斤拷锟斤拷锟劫讹拷矢锟斤拷锟角讹拷
	END_POS leg_end[3];
	END_POS tri_scale[3];
	char i = 0, j = 0;

	for (i = 0; i < 4; i++)
	{
		if (i != per_trig_id)
		{
			leg_end[j++] = vmc[i].epos_nn;
		}
	}

	cal_tri_scale_point(leg_end[0], leg_end[1], leg_end[2], &tri_scale[0], &tri_scale[1], &tri_scale[2], walk_gait.stable_band*0.8);

#if 1
	tar_cog_pos = cal_tri_cog(leg_end[0], leg_end[1], leg_end[2]);
	tar_cog_pos.x *= scale_p[0];
	tar_cog_pos.y *= scale_p[1];
#else
	tar_cog_pos = cal_tri_cog1(leg_end[0], leg_end[1], leg_end[2]);
#endif
	//printf("spd_yaw=%f %f %f\n", spd_yaw, tar_cog_pos.x, tar_cog_pos.y);
	return tar_cog_pos;
}


END_POS cog_planner_per_tirg_n(char per_trig_id)//预锟斤拷锟斤拷时锟斤拷COG锟芥划 全锟斤拷
{
	END_POS vector_spd;
	END_POS tar_cog_pos, tar_cog_pos1, tar_cog_pos2;
	float tar_dx = vmc_all.param.tar_spd_use_rc.x;
	float tar_dy = vmc_all.param.tar_spd_use_rc.y;
	float spd = sqrt(tar_dx*tar_dx + tar_dy * tar_dy);
	float spd_yaw = cal_yaw_xy(tar_dx, tar_dy);//锟斤拷锟斤拷锟劫讹拷矢锟斤拷锟角讹拷
	END_POS leg_end[3];
	END_POS tri_scale[3];
	char i = 0, j = 0;
	for (i = 0; i < 4; i++)
	{
		if (i != per_trig_id)
		{
			leg_end[j++] = vmc[i].epos_nn;//globale
		}
	}

	cal_tri_scale_point(leg_end[0], leg_end[1], leg_end[2], &tri_scale[0], &tri_scale[1], &tri_scale[2], walk_gait.stable_band*0.8);

#if 1
	tar_cog_pos = cal_tri_cog(leg_end[0], leg_end[1], leg_end[2]);
#else
	tar_cog_pos1 = cal_tri_cog(leg_end[0], leg_end[1], leg_end[2]);
	tar_cog_pos2 = cal_tri_cog1(leg_end[0], leg_end[1], leg_end[2]);
	float w1 = 0.8;
	tar_cog_pos.x = tar_cog_pos1.x*w1 + (1 - w1)*tar_cog_pos2.x;
	tar_cog_pos.y = tar_cog_pos1.y*w1 + (1 - w1)*tar_cog_pos2.y;
#endif
	return tar_cog_pos;
}

//选锟斤拷锟斤拷要锟斤拷锟斤拷
char find_leg_need_move(void) {
	END_POS vector_spd;
	float tar_dx = vmc_all.param.tar_spd_use_rc.x;
	float tar_dy = vmc_all.param.tar_spd_use_rc.y;
	float spd = sqrt(tar_dx*tar_dx + tar_dy * tar_dy);
	float spd_yaw = cal_yaw_xy(tar_dx, tar_dy);//锟斤拷锟斤拷锟劫讹拷矢锟斤拷锟角讹拷
	char id_most_far = 99;//锟劫度凤拷锟斤拷锟斤拷蠓降慕锟?	float k1,b1;
	char i = 0, j = 0, k = 0, l = 0;
	char num = 0;
	float id_buf[4];
	float id_buf_dis[4];
	float id_buf_space[4];
	float id_buf_dis_space[4];
	float dis_most_way = 0;
	char id_can_trig[4] = { 99,99,99,99 };
	char leg_check_state[4] = { 99,99,99,99 };
	END_POS pos_check[3];
	float k1, b1;
	vector_spd = walk_gait.now_zmp_n; j = 0;
	line_function90_from_arrow(walk_gait.now_zmp_n.x, walk_gait.now_zmp_n.y, spd_yaw, &k1, &b1);//锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷俣锟绞革拷锟斤拷锟斤拷锟?
	if (spd > MIN_SPD_ST || 1) {
		l = 0;

		for (i = 0; i < 4; i++)
		{
			if (vmc[i].ground)//锟斤拷锟斤拷支锟斤拷锟斤拷
			{
				//去锟斤拷锟较达拷锟狡讹拷锟斤拷
				if (i == walk_gait.trig_id_his[0] || (i == walk_gait.trig_id_his[1] && 1)) {
					leg_check_state[i] = 1;
					continue;
				}

				//去锟斤拷锟斤拷锟斤拷失锟饺碉拷锟斤拷
				k = 0;
				for (j = 0; j < 4; j++)
				{
					if (j != i)
						pos_check[k++] = vmc[j].epos_n;
				}
				//			  if(!inTrig(walk_gait.now_zmp_n,pos_check[0],pos_check[1],pos_check[2])){
				//					leg_check_state[i]=2;
				//					continue;}

				id_can_trig[l++] = i;//锟斤拷录锟斤拷锟狡讹拷锟斤拷
			}
		}
		if (l == 1)
			return id_can_trig[0];

		id_most_far = 99;
		dis_most_way = 0;
		END_POS jiao[4][2];
		char flag_temp = 0;
		for (i = 0; i < l; i++) {//统锟狡革拷锟斤拷锟斤拷锟劫度凤拷锟斤拷锟较可讹拷锟斤拷围
			flag_temp = check_point_to_tangle_n(
				vmc[id_can_trig[i]].epos.x,
				vmc[id_can_trig[i]].epos.y,
				spd_yaw,
				MAX_X + vmc[id_can_trig[i]].flag_fb*-vmc_all.param.param_vmc.side_off_stand[Xr],
				MAX_Y + vmc[id_can_trig[i]].flag_rl*-vmc_all.param.param_vmc.side_off_stand[Yr],
				MIN_X + vmc[id_can_trig[i]].flag_fb*-vmc_all.param.param_vmc.side_off_stand[Xr],
				MAX_Y + vmc[id_can_trig[i]].flag_rl*-vmc_all.param.param_vmc.side_off_stand[Yr],
				MIN_X + vmc[id_can_trig[i]].flag_fb*-vmc_all.param.param_vmc.side_off_stand[Xr],
				MIN_Y + vmc[id_can_trig[i]].flag_rl*-vmc_all.param.param_vmc.side_off_stand[Yr],
				MAX_X + vmc[id_can_trig[i]].flag_fb*-vmc_all.param.param_vmc.side_off_stand[Xr],
				MIN_Y + vmc[id_can_trig[i]].flag_rl*-vmc_all.param.param_vmc.side_off_stand[Yr]
				, &jiao[i][0].x, &jiao[i][0].y, &jiao[i][1].x, &jiao[i][1].y);
			if (flag_temp) {
				id_buf_dis[i] = cal_dis_of_points(
					vmc[id_can_trig[i]].epos.x,
					vmc[id_can_trig[i]].epos.y,
					jiao[i][0].x, jiao[i][0].y);

				if (id_buf_dis[i] > dis_most_way) {
					id_most_far = id_can_trig[i];
					dis_most_way = id_buf_dis[i];
				}
			}
		}

		return id_most_far;
	}
	else
		return 99;
}


void  Gait_Walk_Active(void)
{
	char i = 0;
	GAIT_SHEC gait_zero;
	walk_gait.state_m = 0;
	vmc_all.param.robot_mode = M_WALK;
	vmc_all.gait_mode = WALK;
	vmc_all.tar_att[YAWr] = vmc_all.att_ctrl[YAWr];
	vmc_all.tar_pos.z = 0.77*-MAX_Z;// vmc_all.pos_n.z;
	slip_mode.slip_F[Xr] = slip_mode.slip_F[Yr] = slip_mode.slip_F[Zr] = 0;
	slip_mode.slip_timer = 0;
#if RUN_PI
	vmc_robot_p.ground_mu = config_gait["walk_param"]["ground_mu"].as<float>();
#endif
	robotwb.exp_att.yaw = robotwb.now_att.yaw;
	for (i = 0; i < 4; i++) {
		vmc[i].tar_sita1 = vmc[i].sita1;
		vmc[i].tar_sita2 = vmc[i].sita2;
		vmc[i].tar_sita3 = vmc[i].sita3;
		vmc[i].ground = 1;
		vmc_all.param.trot_sw_end_flag[i] = 0;
		vmc_all.param.gait_sw_end_reg_n[i] = vmc[i].epos_n;
		vmc[i].st_phase = 0;
		reset_tar_pos(i);
	}
	if (vmc_all.ground_num >= 2 && stand_force_enable_flag[4] && USE_FPOS_CONTROL && !TEST_TROT_SW) {//使能力控
		stand_force_enable_flag[0] = 1;
		stand_force_enable_flag[1] = 1;
		stand_force_enable_flag[2] = 1;
		stand_force_enable_flag[3] = 1;
		stand_force_enable_flag[4] = 1;
		vmc_all.trot_air_test = 0;
		for (i = 0; i < 4; i++) {
			gait.ground_flag[i][1] = gait.ground_flag[i][0] = 1;
		}
	}
	else {
		for (i = 0; i < 4; i++) {
			gait.ground_flag[i][1] = gait.ground_flag[i][0] = 0;
		}
		vmc_all.trot_air_test = 1;
	}
#if RUN_WEBOTS||RUN_PI
	if (vmc_all.trot_air_test)
		printf("Walk Gait Initing::Air Test!!!!!!!\n");
	else
		printf("Walk Gait Initing::Gait Mode!!!!!!!\n");
#endif

	reset_servo_interge();
#if RUN_PI
	kf_pos_vel.rst_kf = 1;
	kf_pos_vel.force_ground = 0;
	moco8_ml();//reset param
#endif


#if RUN_PI
	vmc_all.param.param_vmc_default.stance_time[0] = config_gait["walk_param"]["swing_time"].as<float>();//0.4;//摆动时间
	vmc_robot_p.sw_deltah = config_gait["walk_param"]["swing_height"].as<float>();//0.0525;//摆动高度
	pos_force_p.touch_z_param_sw.st_td = config_gait["walk_param"]["sw_td"].as<float>();//10.0;//N  SW
	pos_force_p.touch_z_param_sw.trot_sw = config_gait["walk_param"]["trot_sw"].as<float>();//10.0;//N  SW
	vmc_all.gait_time[0] = vmc_all.gait_time[1] = vmc_all.param.param_vmc_default.stance_time[0];//???????

	walk_gait.imp_force_time_rate = config_gait["walk_param"]["imp_force_time_rate"].as<float>();//10.0;//N  SW
	walk_gait.imp_alfa[Xr] = config_gait["walk_param"]["imp_alfa_x"].as<float>();//10.0;//N  SW
	walk_gait.imp_alfa[Zr] = config_gait["walk_param"]["imp_alfa_z"].as<float>();//10.0;//N  SW
	gait = gait_zero;
#else

	walk_gait.imp_force_time_rate = 0.3;
	walk_gait.imp_alfa[Xr] = 0.06;
	walk_gait.imp_alfa[Zr] = 50;
#endif
	gait.timer[0] = gait.timer[1] = 0;
	walk_init = 0;
	for (i = 0; i < 4; i++) {
		gait.imp_force[i][Zr] = 0;
		gait.state[i] = 0;
	}


	vmc_all.cog_pos_n.x = vmc_all.cog_pos_n.y = 0;
	vmc_all.zmp_pos_n.x = vmc_all.zmp_pos_n.y = 0;
	vmc_all.cog_spd_n.x = vmc_all.cog_spd_n.y = vmc_all.cog_spd_n.z = 0;
	vmc_all.zmp_spd_n.x = vmc_all.zmp_spd_n.y = vmc_all.zmp_spd_n.z = 0;

	walk_gait.tar_zmp_n.x = walk_gait.tar_zmp_n.y = walk_gait.tar_zmp_n.z = 0;
	walk_gait.tar_zmp_b.x = walk_gait.tar_zmp_b.y = walk_gait.tar_zmp_b.z = 0;
	walk_gait.now_zmp_n.x = walk_gait.now_zmp_n.y = walk_gait.now_zmp_n.z = 0;
	walk_gait.now_zmp_b.x = walk_gait.now_zmp_b.y = walk_gait.now_zmp_b.z = 0;

	walk_gait.trig_id_last = 99;
	walk_gait.trig_id_his[0] = walk_gait.trig_id_his[1] = walk_gait.trig_id_his[2] = walk_gait.trig_id_his[3] = 99;
	walk_gait.xy_control_mode = 1;//锟饺讹拷锟斤拷锟斤拷
	walk_gait.stable_value = 0;
	walk_gait.cog_stable = 0;
	walk_gait.cog_stable_timer = 0;
	walk_gait.min_st_value = 0.005;//use
	walk_gait.stable_band = 0.9;//use
	walk_gait.cog_reach_dead = 0.05;//锟斤拷锟侥匡拷锟斤拷锟斤拷锟?
	walk_gait.work_space_band = 0.95;//锟斤拷锟斤拷锟斤拷锟斤拷呓锟?
	walk_gait.min_support_area = 0.4;//use
	walk_gait.cog_reach_deadnn = 0.035;//锟斤拷锟侥匡拷锟斤拷锟斤拷锟?全锟斤拷
	walk_gait.sw_h_off_walke = 0.035;//walke模式锟斤拷抬锟斤拷

		//map 初始化地图
	global_map.init_odom = 1;
#if !FMAP_USE_SIMU
	fake_map_init(&fake_map_trench_5cm, MAP_H*MAP_SIZE*0.5*-1, 0, 0, 0);
#endif
}


float vmc_all_gait_alfa = 0.25;
//float S_walk[4] = { 0,1,1,0 };		 //FR HR FL HL  摆动滞后比例  优先支撑
float S_walk[4] = { 0,3,2,1 };		 //FR HR FL HL
//float S_walk[4] = { 0,3-0.0 -0.3,2 - 0.3,1-0.0 };
void phase_gait_schedual(GAIT_SHEC* in, float dt)
{
	char i, j;
	float biggest_S = 0;
	char  id_biggest = in->ref_leg;
	float max_delay[2] = { 0,0 };
	if (!walk_init)
	{
		walk_init = 1;
		in->T_sw = vmc_all.gait_time[0] - vmc_all.stance_time;
		in->T_st = 0.25;
		in->T_sw = 0.15;
		in->t_gait_change = vmc_all.gait_time[0] * 3;
		in->gait_mode[0] = 0;
		in->w_gait_change = 0.03;
		in->min_st_rate = 0.6;
		in->ref_leg = 0;
		for (i = 0; i < 4; i++) {
			in->ground_flag[i][1] = in->ground_flag[i][0] = 1;
			in->S_now[i] = in->S_target[i] = S_walk[i];
		}
	}
#if 1
	in->T_sw = vmc_all.gait_time[1] - vmc_all.stance_time;
	//printf("in->T_sw=%f\n",vmc_all.gait_time[1]);
	in->T_st = in->T_sw*(vmc_all_gait_alfa * 2);
	vmc_all.delay_time[0] = in->T_st * 2;
#else
	in->T_sw = (vmc_all.gait_time[1] - vmc_all.stance_time)*vmc_all_gait_alfa;
	in->T_st = vmc_all.gait_time[1] - vmc_all.stance_time - in->T_sw;
#endif
	in->timer[0] += dt;//system timer 一直递增的计数器


	  //计算该次步态相序所有时间
	for (i = 0; i < 4; i++)
		if (max_delay[0] < in->S_now[i]) {
			max_delay[0] = in->S_now[i];//摆动最大滞后比例
			max_delay[1] = i;
		}

	in->T_all = (in->T_st + in->T_sw)*max_delay[0] + in->T_sw + in->T_st;

	for (i = 0; i < 4; i++) {
		in->sw_start[i] = in->S_now[i] * (in->T_st + in->T_sw);
		in->sw_end[i] = in->sw_start[i] + in->T_st;
	}

	for (i = 0; i < 4; i++)
		in->S_sw[i] = in->S_st[i] = 0;//标志清0
//----------------------------td tirg-----------------------
	if (in->t_ref > 0.98*in->T_all)//参考腿上一次摆动完成百分比
	{
		in->t_d = in->timer[0];
	}//本次步态参考时间清零

	if (in->timer[1] > in->timer[0])//总时间计数器溢出保护
	{
		in->t_d = in->timer[0];
	}

	in->t_ref = LIMIT(in->timer[0] - in->t_d, -in->T_all, in->T_all);//参考时间以上次结束时间清0
 //---------------------------gait change-----------------------------
	in->gait_mode[0] = vmc_all.gait_mode;
	switch (in->gait_mode[0]) {
		//case TROT://tort
		   // 	for(i=0;i<4;i++)
		   //	 in->S_target[i]=S_tort[i];
		//break;
	case WALK://walk
		in->S_target[i] = S_walk[i];  //forward
		break;
	}


	for (i = 0; i < 4; i++)
	{
		in->S_now[i] = in->S_target[i] * in->w_gait_change + (1 - in->w_gait_change)*in->S_now[i];
	}
	//			{
	//			in->w_gait_change=LIMIT(fabs(0.175)-fabs(vmc_all.tar_spd.x),0,0.175)/0.175;
	//			in->S_now[i]=S_walk[i]*in->w_gait_change+(1-in->w_gait_change)*S_tort[i];}
	//------------------------------gait planner--------------------------------
	for (i = 0; i < 4; i++) {//以支撑开始 S->0表示立即支撑
		in->ti[i] = LIMIT(in->t_ref, 0, in->T_all);//具有延时的各腿参考时间
		//stance  参考时间大于0则支撑
		if (in->ti[i] > in->sw_start[i] && in->ti[i] <= in->sw_end[i]) {
			in->S_st[i] = 0;
			in->S_sw[i] = 1;
		}
		else
		{
			in->S_st[i] = 1;
			in->S_sw[i] = 0;
		}
	}
	//----------------------------------gait flag-------------------------------
	for (i = 0; i < 4; i++) {
		//stance
		if (in->S_st[i] > 0.005)
			in->ground_flag[i][0] = 1;
		//swing
		if (in->S_sw[i] > 0.005)
			in->ground_flag[i][0] = 0;
		//reg
		in->ground_flag[i][1] = in->ground_flag[i][0];
	}
	//----------------------------------reg-------------------------------
	in->gait_mode[1] = in->gait_mode[0];
	in->timer[1] = in->timer[0];
	for (i = 0; i < 4; i++)
	{
		if (in->S_now[i] > biggest_S)
		{
			biggest_S = in->S_now[i];
			id_biggest = i;
		}
	}

	in->S_sw_ref_reg = in->S_sw[(int)max_delay[1]];//最大滞后腿摆动完成比例 作为结束相序参考
}

float cal_BezierP3_w(float s, float a, float b, float c, float d)//三阶贝塞尔
{
	float temp = 1 - limitw(s, 0, 1);
	float o_temp = a * temp* temp * temp + 3 * b * s * temp * temp + 3 * c * s * s * temp + d * s * s * s;
	return o_temp;
}

float cal_Impluse_Fz_w(float alfa, float s_st, float s_peak)//Z方力前馈
{
	float fz;
	float s1, s2;

	if (s_st <= s_peak) {
		s1 = s_st / s_peak;
		fz = alfa * cal_BezierP3_w(s1, 0, 0.8, 1, 1);
	}
	else {
		s2 = (s_st - s_peak) / (1 - s_peak);
		fz = alfa * cal_BezierP3_w(s2, 1, 1, 0.8, 0);

	}
	return fz;
}

#define USE_IMP_PLAN 0
void  Gait_Walk_Update_v1(float dt)
{
	char switch_id[4] = { 0,3,2,1};
	char trig_state_flag[4] = { 0,0,0,0 };
	static char init[4], init_lisence = 0, state, cnt_time_change, switch_flag;
	static float time_delay[4], yaw_force, time_delay_all;
	static float timer_task[10];
	char i, j, sel;
	float test_epos_b[3], test_epos_leg[3];
	static END_POS end_pos_n[4], end_pos_n_use[4];//ĩ�˹̶���ȫ������ϵ�µ�λ��
	static float cnt_mode_can_switch = 0, mode_en_switch_flag = 0;
	static float att_test_use[3];
	char leg_sel_trig = 0;
	static float en_force_timer[3] = { 0 };
	float att_off[2];
	static float phast_time[4] = { 0 };
	static float phast_time_over[4] = { 0 };
	static int first_trig = 0;
	static float cog_reach_cnt = 0;
	static char trig_id = 0;
	    //char switch_id_f[4] = { 3,2,1,0};//
	    //char switch_id_b[4] = { 0,1,2,3};//
	char switch_id_f[4] = { 3,1,0,2 };//
	char switch_id_b[4] = { 0,2,1,3 };//
	char ukey_state = check_lisence();

	float min_space = 0;
	float cog_err = 0;
	END_POS tri_fast_move_buf[3];
	static float cnt_walk_gait = 0;

	float pos_gain_all = 1;
#if FMAP_USE_SIMU
	fake_map_link_init();
#else
	fake_map_init(&fake_map_trench_5cm, 0, 0, 0, 0);
#endif

	move_robot_on_map_n(dt);//移动地图

	if (timer_task[2] > 0.02) {
		body_traj_planner(timer_task[2]);
		timer_task[2] = 0;
	}

	if (walk_gait.state_m == 5) {
		one_leg_trig_id++;
	}
	if (one_leg_trig_id >= 4)
		one_leg_trig_id = 0;

	//phase_gait_schedual(&gait, dt);
	//printf("%d\n",walk_gait.state_m);
	switch (walk_gait.state_m)
	{
	case 0:
		//            if((vmc_all.unmove==0&&vmc_all.hand_hold==0&&vmc_all.fly==0&&//walk_gait.need_trig&&
		//             (ABS(vmc_all.tar_spd.x)>MIN_SPD_ST*vmc_all.param.move_check_x
		//            ||ABS(vmc_all.tar_spd.z)>MIN_SPD_ST*vmc_all.param.move_check_x
		//            ||ABS(vmc_all.tar_spd.y)>MIN_SPD_ST*vmc_all.param.move_check_x
		//            ||1))||(ONE_LEG_TRIG_TEST&&one_leg_trig_id<=3)){
		if (1) {

			if (cnt_time_change++ > 2 || 1) {//WS
				vmc_all.gait_time[1] = vmc_all.gait_time[3];
				vmc_all.stance_time = vmc_all.stance_time_auto;
				vmc_all.delay_time[2] = vmc_all.delay_time[1];
				cnt_time_change = 0;
				vmc_all.param.is_trap = 0;
			}

#if !WALK_SECH_ROUND||1
			vmc_all.param.tar_spd_use_rc.x = 0.1;
			if (fabs(vmc_all.param.tar_spd_use_rc.x) < MIN_SPD_ST&&fabs(vmc_all.param.tar_spd_use_rc.y) < MIN_SPD_ST)
				trig_id = switch_id[LIMIT(one_leg_trig_id, 0, 3)];//find leg to swing
			else {
				if (one_leg_trig_id != 99)
					trig_id = find_leg_need_move();//寻锟揭摆讹拷锟斤拷
			}
#else
			trig_id = switch_id_f[LIMIT(one_leg_trig_id, 0, 3)];//open loop
#endif
			if (trig_id <= 3 && vmc_all.param.stand_trot_switch_flag < 3) {//
				walk_gait.pre_trig_id = trig_id;//
				walk_gait.need_trig = 0;
				walk_gait.xy_control_mode = 1;//
				walk_gait.state_m++;
				walk_gait.leg_move_per_trig = vmc[walk_gait.pre_trig_id].epos_n;
				cnt_walk_gait = 0;
				walk_gait.sw_cog_st_reg= vmc_all.cog_pos_n;
			}
			printf("Trig leg=%d\n", trig_id);
		}
		break;
	case 1://control cog to ZMP
	//------cal tar zmp
#if FAKE_EXP_ZMP_TEST&&!ONE_LEG_TRIG_TEST//锟斤拷锟侥规划
		walk_gait.tar_zmp_n = tar_zmp_fake;
#else
		walk_gait.tar_zmp_n = cog_planner_per_tirg(walk_gait.pre_trig_id);//跟换全局
		walk_gait.att_com_off[PITr] = 0;
		walk_gait.att_com_off[ROLr] = 0;
#endif
		//------cal tar attitude
//        vmc_all.tar_att_off[PITr]=  att_per_trig[PITr]*vmc[walk_gait.pre_trig_id].flag_fb;
//        vmc_all.tar_att_off[ROLr]= -att_per_trig[ROLr]*vmc[walk_gait.pre_trig_id].flag_rl;

		cnt_walk_gait += dt;
		cog_err = sqrt(
	      pow(walk_gait.tar_zmp_n.x - walk_gait.now_zmp_nn.x, 2) * WALK_USE_X
		+ pow(walk_gait.tar_zmp_n.y - walk_gait.now_zmp_nn.y, 2) *(vmc_all.param.leg_dof == 3));
	   // printf("cog=%f %f tar=%f %f\n",walk_gait.now_st_cog_n1.x,walk_gait.now_st_cog_n1.y,walk_gait.tar_zmp_n.x,walk_gait.tar_zmp_n.y);
		// printf("NN cog=%f %f tar=%f %f\n",vmc_all.zmp_pos_n.x,vmc_all.zmp_pos_n.y,walk_gait.tar_zmp_nn.x,walk_gait.tar_zmp_nn.y);

		if (cog_err > walk_gait.cog_reach_dead)
			walk_gait.cog_tar_reach = cog_reach_cnt = 0;

		//double trigger
		walk_gait.en_fast_cog_move = 0;
		if (1 && walk_gait.tar_spd_length > MIN_SPD_ST &&
			!check_point_front_arrow_n(vmc[walk_gait.pre_trig_id].epos_n, walk_gait.now_zmp_n, walk_gait.tar_spd_yaw))//
		{
			j = 0;
			for (i = 0; i < 4; i++)
			{
				if (i != walk_gait.pre_trig_id)
				{
					tri_fast_move_buf[j++] = vmc[i].epos_n;
				}
			}

			if (inTrig(walk_gait.now_zmp_n, tri_fast_move_buf[0], tri_fast_move_buf[1], tri_fast_move_buf[2]))//
				walk_gait.en_fast_cog_move = 1;
		}


		if (((walk_gait.cog_stable//ZMP>0
			&&walk_gait.cog_tar_reach && !FAKE_EXP_ZMP_TEST) ||
			(cnt_walk_gait > vmc_all.gait_time[0]*0.25 && cog_err < walk_gait.cog_reach_dead*time_reach_scale) ||
			walk_gait.en_fast_cog_move) && vmc_all.param.stand_trot_switch_flag != 4
			&& 1) {//reach zmp & start to swing

			min_space = 0;
			for (j = 0; j < 4; j++)
			{
				if (vmc[j].ground)
					if (walk_gait.work_space[j] > min_space&&walk_gait.work_space[j] > 0)
						min_space = walk_gait.work_space[j];
			}
			if (min_space == 0 || walk_gait.area_too_small)min_space = 0;//

			if (vmc_all.param.stand_trot_switch_flag == 2)//
				walk_gait.walk_cmd_trig.x = walk_gait.walk_cmd_trig.y = 0;
			else {//
				walk_gait.walk_cmd_trig.x = LIMIT((vmc_all.param.tar_spd_use_rc.x - vmc_all.cog_spd_n.x)*k_walk_trig_spd[Xr], -fabs(min_space)*0.3, fabs(min_space) * 1);
				walk_gait.walk_cmd_trig.y = LIMIT((vmc_all.param.tar_spd_use_rc.y - vmc_all.cog_spd_n.y)*k_walk_trig_spd[Yr], -fabs(min_space)*0.8, fabs(min_space)*0.8);
			}

			walk_gait.leg_off_move.x = vmc[walk_gait.pre_trig_id].epos_n.x - walk_gait.leg_move_per_trig.x;
			walk_gait.leg_off_move.y = vmc[walk_gait.pre_trig_id].epos_n.y - walk_gait.leg_move_per_trig.y;
			walk_gait.leg_off_move.z = vmc[walk_gait.pre_trig_id].epos_n.z - walk_gait.leg_move_per_trig.z;

			walk_gait.trig_id = walk_gait.pre_trig_id;

			trig_plan(walk_gait.trig_id, dt);
			vmc[walk_gait.trig_id].param.trig_state = 1;
			vmc[walk_gait.trig_id].ground = 0;
			stand_force_enable_flag[walk_gait.trig_id] = 0;
			walk_gait.state_m++;
			walk_gait.sw_off.x=vmc_all.cog_pos_n.x - walk_gait.sw_cog_st_reg.x;
			walk_gait.sw_off.y = vmc_all.cog_pos_n.y - walk_gait.sw_cog_st_reg.y;
			printf("COG reach =%f %f leg=%f %f!\n", walk_gait.sw_off.x,walk_gait.sw_off.y, walk_gait.leg_off_move.x, walk_gait.leg_off_move.y);
		}

		break;
	case 2://lift
		trig_state_flag[walk_gait.trig_id] = trig_lift(walk_gait.trig_id, dt);
		if (trig_state_flag[walk_gait.trig_id]) {
			vmc[walk_gait.trig_id].param.trig_state = 2;
			walk_gait.state_m++;
		}
		break;
	case 3://sw
		switch (vmc[walk_gait.trig_id].param.trig_state)
		{

		case 2:
			trig_state_flag[walk_gait.trig_id] = trig_swing(walk_gait.trig_id, dt);
			if (trig_state_flag[walk_gait.trig_id] == 1)//锟叫伙拷支锟斤拷
				vmc[walk_gait.trig_id].param.trig_state = 3;
			else if (trig_state_flag[walk_gait.trig_id] == 2)//锟叫伙拷TD
				vmc[walk_gait.trig_id].param.trig_state = 4;
			break;
		case 3:
			trig_state_flag[walk_gait.trig_id] = trig_td(walk_gait.trig_id, dt);
			if (trig_state_flag[walk_gait.trig_id] >= 1)//锟叫伙拷支锟斤拷
				vmc[walk_gait.trig_id].param.trig_state = 4;
			break;
		}

#if GROUND_AFTER_TRIG
		if (vmc[walk_gait.trig_id].param.trig_state == 4)
		{
			vmc[walk_gait.trig_id].ground = 1;
			stand_force_enable_flag[walk_gait.trig_id] = 1;
			vmc[walk_gait.trig_id].param.trig_state = 0;
#else
		if (vmc[walk_gait.trig_id].ground)
		{
#endif

			if (vmc_all.delay_time[2] > 0 || ONE_LEG_TRIG_TEST)
				walk_gait.state_m++;
			else
			{
				walk_gait.state_m = 5;
				walk_gait.trig_id_last = walk_gait.trig_id;
				walk_gait.trig_id_his[3] = walk_gait.trig_id_his[2];
				walk_gait.trig_id_his[2] = walk_gait.trig_id_his[1];
				walk_gait.trig_id_his[1] = walk_gait.trig_id_his[0];
				walk_gait.trig_id_his[0] = walk_gait.trig_id;
			}


			walk_gait.xy_control_mode=0;

			if(vmc_all.param.stand_trot_switch_flag==2)
			    vmc_all.param.stand_switch_cnt[0]++;
			if(vmc_all.param.stand_switch_cnt[0]>=4)
			    vmc_all.param.stand_trot_switch_flag=3;
		}
		break;
	case 4://锟斤拷锟斤拷锟脚碉拷锟斤拷时
		time_delay_all += dt;
#if ONE_LEG_TRIG_TEST
		vmc_all.delay_time[2] = walk_sw_time;
#endif
		if (time_delay_all > vmc_all.delay_time[2] && 1)
		{
			time_delay_all = 0;
			walk_gait.state_m++;
			walk_gait.trig_id_last = walk_gait.trig_id;

			walk_gait.trig_id_his[3] = walk_gait.trig_id_his[2];
			walk_gait.trig_id_his[2] = walk_gait.trig_id_his[1];
			walk_gait.trig_id_his[1] = walk_gait.trig_id_his[0];
			walk_gait.trig_id_his[0] = walk_gait.trig_id;

			walk_gait.xy_control_mode = 0;
		}
		break;
	case 5:
		walk_gait.state_m = 0;
		break;
		}

	for (i = 0; i < 4; i++) {//输出摆动速度
		if (vmc[i].param.trig_state >= 1 && vmc[i].param.trig_state <= 3)//
			swing_spd_control(i, dt);
		if (vmc[i].ground)
			vmc[i].st_phase += dt;
		else
			vmc[i].st_phase = 0;
	}


	if (walk_gait.xy_control_mode == 1 || 1) {//-----COG spd control
		   //cog锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷
		float tar_spd_walk[2];
		tar_spd_walk[Xr] =
			LIMIT(walk_gait.tar_zmp_n.x - walk_gait.now_zmp_nn.x, -0.1, 0.1)*cog_pid[Xr] * pos_gain_all
			- vmc_all.cog_spd_n.x*cog_pidd[Xr];
		tar_spd_walk[Yr] =
			LIMIT(walk_gait.tar_zmp_n.y - walk_gait.now_zmp_nn.y, -0.1, 0.1)*cog_pid[Yr] * pos_gain_all
			- vmc_all.cog_spd_n.y*cog_pidd[Yr];
		//printf("tar=%f %f now=%f %f\n", walk_gait.tar_zmp_n.x, walk_gait.tar_zmp_n.y, walk_gait.now_zmp_nn.x, walk_gait.now_zmp_nn.y);
		tar_spd_walk[Xr] *= (0.01+0.19* WALK_USE_X);
		tar_spd_walk[Yr] *= 0.2;


		//convert to body
		vmc_all.tar_spd_walk[Xr] = tar_spd_walk[Xr] * cosd(-vmc_all.ground_att[YAWr]) - tar_spd_walk[Yr] * sind(-vmc_all.ground_att[YAWr]);// +vmc_all.cog_pos_n.x;//???????
		vmc_all.tar_spd_walk[Yr] = tar_spd_walk[Xr] * sind(-vmc_all.ground_att[YAWr]) + tar_spd_walk[Yr] * cosd(-vmc_all.ground_att[YAWr]);// +vmc_all.cog_pos_n.y;
		//                vmc_all.FALL[Xr]*=pit_w*rol_w;
		//                vmc_all.FALL[Yr]*=pit_w*rol_w;
		vmc_all.tar_spd_walk[Xr] += 0.1 * !WALK_USE_X;
		if (sqrt(
			  pow(walk_gait.tar_zmp_n.x - walk_gait.now_zmp_nn.x, 2) * WALK_USE_X
			+ pow(walk_gait.tar_zmp_n.y - walk_gait.now_zmp_nn.y, 2) *(vmc_all.param.leg_dof == 3)) < walk_gait.cog_reach_dead)
			cog_reach_cnt += dt;
		else
			walk_gait.cog_tar_reach = cog_reach_cnt = 0;

		if (cog_reach_cnt > cog_reach_time_check)
			walk_gait.cog_tar_reach = 1;

		//printf("tar_spd=%f %f [%d] z=%f\n",vmc_all.tar_spd_walk[Xr],vmc_all.tar_spd_walk[Yr],walk_gait.cog_tar_reach,vmc_all.cog_pos_n.z);
	  // vmc_all.FALL_Interge[Xr]=vmc_all.FALL_Interge[Yr]=0;
	}
	else if (walk_gait.cog_stable) {//COG锟饺讹拷时锟斤拷锟劫度匡拷锟斤拷

//                if(fabs(vmc_all.param.tar_spd_use_rc.x)>MIN_SPD_ST)
//                    vmc_all.FALL_Interge[Xr]+=(vmc_all.param.tar_spd_use_rc.x-vmc_all.cog_spd_n.x)*vmc_all.param.param_vmc.pid_vx[1]*dt*vmc_all.param.gain_control_x;
//                else
//                    vmc_all.FALL_Interge[Xr]=0;

//                if(fabs(vmc_all.param.tar_spd_use_rc.y)>MIN_SPD_ST&&vmc_all.param.leg_dof==3)
//                    vmc_all.FALL_Interge[Yr]+=(vmc_all.param.tar_spd_use_rc.y-vmc_all.cog_spd_n.y)*vmc_all.param.param_vmc.pid_vy[1]*dt*vmc_all.param.gain_control_x;
//                else
//                    vmc_all.FALL_Interge[Yr]=0;

//                vmc_all.FALL[Xr]=
//                (vmc_all.param.tar_spd_use_rc.x-vmc_all.cog_spd_n.x)*vmc_all.param.param_vmc.pid_vx[0]*!vmc_all.unmove*vmc_all.param.gain_control_x
//                +vmc_all.T_FP[Xr]+vmc_all.FALL_Interge[Xr];//前锟斤拷
//                vmc_all.FALL[Yr]=
//                (vmc_all.param.tar_spd_use_rc.y-vmc_all.cog_spd_n.y)*vmc_all.param.param_vmc.pid_vy[0]*!vmc_all.unmove*vmc_all.param.gain_control_x
//                +vmc_all.T_FP[Yr]+vmc_all.FALL_Interge[Yr];//锟斤拷锟斤拷
//                vmc_all.FALL[Yr]*=(vmc_all.param.leg_dof==3);
	}

	//#if 0
	//    if (1) {
	//        if (vmc_all.gait_mode == WALK)//----------------------�Խǲ�̬
	//        {
	//            phase_gait_schedual(&gait, dt);
	//            //event_gait_schedual(&gait, dt);
	//            for (i = 0; i < 4; i++)
	//            {


	//                if (vmc[i].ground)
	//                    vmc[i].param.st_time_used += dt;
	//#if 0
	//                if (gallop_mode.impluse_flag[i]) {
	//                    time_delay[i] += dt;
	//                    if (time_delay[i] > vmc_all.stance_time / (gallop_mode.rate_all[Zr] + 0.000001)) {
	//                        gallop_mode.impluse_flag[i] = 0;
	//                        gallop_mode.sita_dz[i] = 0;
	//                        time_delay[i] = 0;
	//                    }
	//                }
	//#endif
	//                switch (gait.state[i])
	//                {
	//                case 0:


	//                        if (cnt_time_change++ > 4) {
	//                            vmc_all.gait_time[1] = vmc_all.gait_time[3];
	//                            vmc_all.stance_time = vmc_all.stance_time_auto;
	//                            vmc_all.delay_time[2] = vmc_all.delay_time[1];
	//                            cnt_time_change = 0;
	//                        }

	//                        phast_time_over[i] += dt;


	//                        if !WALK_SECH_ROUND
	//                            if(fabs(vmc_all.param.tar_spd_use_rc.x)<MIN_SPD_ST&&fabs(vmc_all.param.tar_spd_use_rc.y)<MIN_SPD_ST)
	//                                trig_id=switch_id[LIMIT(one_leg_trig_id,0,3)];//原锟斤拷锟斤拷转  锟斤拷锟节摆讹拷
	//                            else{
	//                            if(one_leg_trig_id!=99)
	//                                trig_id=find_leg_need_move();//寻锟揭摆讹拷锟斤拷
	//                            }
	//                        #else
	//                            trig_id=switch_id[LIMIT(one_leg_trig_id,0,3)];//锟斤拷锟节摆讹拷
	//                        #endif
	//                        if(trig_id<=3&&vmc_all.param.stand_trot_switch_flag<3){//锟斤拷锟街匡拷锟狡讹拷锟斤拷
	//                              walk_gait.pre_trig_id=trig_id;//预锟斤拷锟斤拷ID
	//                                walk_gait.need_trig=0;
	//                              walk_gait.xy_control_mode=1;//锟斤拷锟斤拷锟斤拷锟斤拷锟饺讹拷
	//                                walk_gait.state_m++;
	//                                walk_gait.leg_move_per_trig=vmc[walk_gait.pre_trig_id].epos_n;
	//                              cnt_walk_gait=0;
	//                        }
	//                    break;
	//                    }


	//                        if (gait.ground_flag[i][0] == 0 && vmc[i].ground == 1
	//                            //&& vmc_all.ground_num>=3
	//                            && (walk_gait.cog_stable||!USE_ZMP|| phast_time_over[i]> vmc_all.stance_time*2)
	//                            )//sech to swing
	//                        {
	//                            vmc[i].param.trig_state = 1;
	//                            phast_time_over[i] = 0;
	//#if USE_IMP_PLAN
	//                            gait.state[i] = 1;//to impluse
	//                            gait.imp_force[i][Zr] = 0;
	//                            time_delay[i] = 0;
	//#else
	//                            trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
	//                            vmc[i].ground = 0;	//清除着地标志位
	//                            stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
	//                            vmc[i].param.trig_state = 1;
	//                            gait.state[i] = 2;//to LF
	//#endif
	//                        }
	//                    }
	//                    break;
	//                case 1://Impluse down
	//#if USE_IMP_PLAN
	//                    time_delay[i] += dt;
	//                    if(i==0||i==2)
	//                        robotwb.exp_pos_n.x = -walk_gait.imp_alfa[Xr] + robotwb.exp_spd_n.x*dt;
	//                    else
	//                        robotwb.exp_pos_n.x = walk_gait.imp_alfa[Xr] + robotwb.exp_spd_n.x*dt;
	//                    gait.imp_force[i][Zr] =cal_Impluse_Fz_w(walk_gait.imp_alfa[Zr], LIMIT(time_delay[i] / (vmc_all.stance_time*walk_gait.imp_force_time_rate), 0, 1), 0.35);
	//                    //printf("i=%d gait.imp_force[i][Zr] =%f\n", i, gait.imp_force[i][Zr]);
	//                    if (time_delay[i] > vmc_all.stance_time*walk_gait.imp_force_time_rate) {
	//                        gait.state[i]++;
	//                        gait.imp_force[i][Zr] = 0;
	//                        time_delay[i] = 0;
	//                        trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
	//                        vmc[i].ground = 0;	//清除着地标志位
	//                        stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
	//                        vmc[i].param.trig_state = 1;
	//                    }
	//#else
	//                    if (1) {
	//                        gait.state[i]++;
	//                        trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
	//                        vmc[i].ground = 0;	//清除着地标志位
	//                        stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
	//                        vmc[i].param.trig_state = 1;
	//                    }
	//#endif
	//                    break;
	//                case 2://LF
	//                    phast_time[i] += dt;
	//                    trig_state_flag[i] = trig_lift(i, dt);

	//                    if (trig_state_flag[i]) {//同时达到？
	//                        vmc[i].param.trig_state = 2;//进入摆动
	//                        gait.state[i]++;
	//                    }

	//                    break;
	//                case 3://SW&TD
	//                    phast_time[i] += dt;

	//                    switch (vmc[i].param.trig_state)
	//                    {
	//                    case 2:
	//                        trig_state_flag[i] = trig_swing(i, dt);

	//                        if (trig_state_flag[i] == 2) {//摆动着地 之前未添加在舵狗
	//                            vmc[i].param.trig_state = 4;
	//                        }
	//                        else if (trig_state_flag[i] == 1)//摆动结束-->TD
	//                            vmc[i].param.trig_state = 3;
	//                        break;
	//                    case 3:
	//                        trig_state_flag[i] = trig_td(i, dt);
	//                        if (trig_state_flag[i] >= 1) {//TD结束正常
	//                            vmc[i].param.trig_state = 4;
	//                        }
	//                        break;
	//                    }


	//#if GROUND_AFTER_TRIG||1
	//                    if ((vmc[i].param.trig_state >= 4)
	//#if FIX_ST_TIME
	//                        &&phast_time[i] > vmc_all.stance_time*MIN_ST_TIME_RATE
	//#endif
	//                        )//对角腿同时着地
	//#else
	//                    if ((vmc[[0]].ground&&vmc[id_sw[1]].ground)
	//#if FIX_ST_TIME
	//                        &&phast_time > vmc_all.stance_time*MIN_ST_TIME_RATE
	//#endif
	//                        )//对角腿同时着地
	//#endif
	//                    {
	//                        if (vmc_all.param.stand_trot_switch_flag == 1) {//站立切换Trot
	//                            vmc_all.param.stand_switch_cnt[0]++;
	//                            vmc_all.param.trot_sw_end_flag[i] = 1;
	//                            if (vmc_all.param.stand_switch_cnt[0] >= 1)//切换完毕
	//                            {
	//                                vmc_all.param.stand_switch_cnt[0] = 0;
	//                                vmc_all.param.stand_switch_flag[1] = 0;//支撑内收
	//                                vmc_all.param.stand_trot_switch_flag = 0;
	//                            }
	//                        }

	//                        if (vmc_all.param.stand_trot_switch_flag == 2) {//Tort切换站立
	//                            vmc_all.param.stand_switch_cnt[1]++;
	//                            switch (vmc_all.param.stand_switch_cnt[1])
	//                            {
	//                            case 1:
	//                                vmc_all.param.stand_switch_flag[1] = 1;//支撑外扩
	//                                break;
	//                            case 3://切换完毕
	//                                vmc_all.param.stand_switch_cnt[1] = 0;
	//                                vmc_all.param.stand_trot_switch_flag = 0;
	//                                break;
	//                            }
	//                        }


	//                        vmc[i].ground = 1;//切换着地符号  非Ground下Load力拉至地面 用于状态估计
	//                        vmc[i].param.trig_state = 0;//复位摆动标志
	//                        stand_force_enable_flag[i] = 1 && !vmc_all.trot_air_test;//启动力控
	//                        vmc[i].param.st_time_used = 0;

	//                        if (cnt_time_change++ > 4) {
	//                            vmc_all.gait_time[1] = vmc_all.gait_time[3];
	//                            vmc_all.stance_time = vmc_all.stance_time_auto;
	//                            vmc_all.delay_time[2] = vmc_all.delay_time[1];
	//                            cnt_time_change = 0;
	//                        }

	//                        if (gait.ground_flag[i][0] == 0 && vmc[i].ground == 1
	//                            && (walk_gait.cog_stable || !USE_ZMP))//sech to swing
	//                        {
	//                            vmc[i].param.trig_state = 1;
	//#if USE_IMP_PLAN
	//                            gait.state[i] = 1;//to impluse
	//                            time_delay[i] = 0;
	//                            gait.imp_force[i][Zr] = 0;
	//#else
	//                            trig_plan(i, dt);//内部			reset_tar_pos(leg_sel_trig);
	//                            vmc[i].ground = 0;	//清除着地标志位
	//                            stand_force_enable_flag[i] = 0;//关闭力控 进入摆动
	//                            vmc[i].param.trig_state = 1;
	//                            gait.state[i] = 2;//to LF
	//#endif
	//                        }
	//                        else
	//                            gait.state[i] = 0;

	//                    }
	//                    break;
	//                }
	//            }//end for
	//#endif

	//            for (i = 0; i < 4; i++) {//输出摆动速度
	//                if (vmc[i].param.trig_state >= 1 && vmc[i].param.trig_state <= 3)//
	//                    swing_spd_control(i, dt);
	//                if (vmc[i].ground)
	//                    vmc[i].st_phase += dt;
	//                else
	//                    vmc[i].st_phase = 0;
	//            }
	//        }
	//    }	//end of lisence check

	for (i = 0; i < 10; i++)
		timer_task[i] += dt;
	}
